/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.archwood.frc2607.logomotion;

import com.sun.squawk.util.StringTokenizer;
import edu.archwood.frc2607.utility.ConsoleLogger;
import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.can.CANTimeoutException;

/**
 *
 * @author Ron
 */
public class LogomotionDriveTrain implements LogomotionConstants {

    private CANJaguar   frontLeftMotor = null, frontRightMotor = null,
                        rearLeftMotor = null, rearRightMotor = null;

    private Gyro            Gyroscope;
    private AnalogChannel   Temperature;
    private DigitalInput    lineTrackerLeft, lineTrackerMid, lineTrackerRight;

    private double      startTemp, currentTemp, currentGyroAngle;
    private double      frontLeftMotorSpeed, frontRightMotorSpeed,
                        rearLeftMotorSpeed, rearRightMotorSpeed;
    private double      startingPosition;
    
    private short       driveNow = 0;

    private ConsoleLogger   cl = ConsoleLogger.getInstance();
    public boolean hasReachedFork = false;
    private int StickWithIt = 0;
    private double DIRECTION = 0;
    private int StopRightHereYouAreReallyAtTheEnd = 0;
    private boolean prevM = false;
private boolean prevL = false;
private boolean prevR = false;
    public boolean endOfLine = false;
    public final void resetGyro() {
        Gyroscope.reset();
    }
    public final void resetLineTracker()
    {
        StickWithIt = 0;
        hasReachedFork = false;
        endOfLine = false;
        StopRightHereYouAreReallyAtTheEnd = 0;
    }
    private void forked()
    {
    if (!hasReachedFork)
                    {
                    hasReachedFork = true;
                    StickWithIt = 5;
                    }
    }
    public void followFork(double speed,double turn,boolean goLeft)
    {
        boolean left, right, mid;
        left = !lineTrackerLeft.get();
        mid = !lineTrackerMid.get();
        right = !lineTrackerRight.get();

        if (mid)//Don't Panic, we are on the line!
        {
            if (left && !right)//If we are leaning towards the left...
            {
                DIRECTION+=turn;
                //speed /= 2;
                if (prevL&&prevR&&prevM)
                {
                    forked();
                }
            }
            if (!left && right)//If we are leaning towards the right...
            {
                DIRECTION-=turn;
                //speed /= 2;
            if (prevL&&prevR&&prevM)
                {
                    forked();
                }
            }
            if (left && right)//If we are at the end...
            {
                //I'll need to know our direction...
                if (hasReachedFork)
                {
                    StopRightHereYouAreReallyAtTheEnd += 1;
                    if (StopRightHereYouAreReallyAtTheEnd >= 3)
                    {
                    endOfLine = true;
                    mecanumDrive(0,0,0);
                    return;
                    }
                }
                else
                {
                  //  DIRECTION+=turn;
                    if (prevL&&prevR&&prevM)
                {
                    forked();
                }
                }
            }
        }
        else
        {
            if (left && !right)//Leaning towards the left a lot?
            {
                DIRECTION+=turn*2;
                //speed /= 2;
                if (prevL&&prevR&&prevM)
                {
                    forked();
                }
            }
            if (!left && right)//Leaning towards the right a lot?
            {
                DIRECTION-=turn*2;
              //speed /= 2;
                if (prevL&&prevR&&prevM)
                {
                    forked();
                }
            }
            if (left && right)//We are at the fork!
            {
                //I'll need to decide... for now, lets go left
              //  DIRECTION+=turn;
              //speed /= 2;
                if (prevL&&prevR&&prevM)
                {
                    forked();
                }
            }
            if (!left && !right)//We are completely lost...
            {
                //isOnLine = no;
                return;
            }
        }
        if (StickWithIt > 0)
        {
            DIRECTION=turn*2;
            if (goLeft)
            {
                DIRECTION = -turn*2;
            }
            StickWithIt-=1;
            System.out.println("All your base are belong to us!");
        }
        mecanumDrive(-speed,0, DIRECTION);
        DIRECTION = 0;
prevL = left; prevM = mid; prevR = right;
    }
    private void initMotors() {
        while (frontLeftMotor == null) {
            try {
                frontLeftMotor = new CANJaguar(frontLeftCANID);
                cl.log("frontLeft Jag created successfully");
            } catch (Exception e) {
                cl.log("frontLeft Jag exception, retrying...");
                Timer.delay(.25);
            }
        }
      
        while (rearLeftMotor == null) {
            try {
                rearLeftMotor = new CANJaguar(rearLeftCANID);
                cl.log("rearLeft Jag created successfully");
            } catch (Exception e) {
                cl.log("rearLeft Jag exception, retrying...");
                Timer.delay(.25);
            }
        }
 
        while (frontRightMotor == null) {
            try {
                frontRightMotor = new CANJaguar(frontRightCANID);
                cl.log("frontRight Jag created successfully");
            } catch (Exception e) {
                cl.log("frontRight Jag exception, retrying...");
                Timer.delay(.25);
            }
        }

        while (rearRightMotor == null) {
            try {
                rearRightMotor = new CANJaguar(rearRightCANID);
                cl.log("rearRight Jag created successfully");
            } catch (Exception e) {
                cl.log("rearRight Jag exception, retrying...");
                Timer.delay(.25);
            }
        }
    }

    private void configMotor(CANJaguar m, String desc, short i) {
        boolean keepTrying = true;
        short entryPoint = 0;
        String msg = " ";
        while (keepTrying) {
            try {
               switch (entryPoint) {
                   case 0:
                       msg = "setting Fault Time";
                       m.configFaultTime(0.5);
                       ++entryPoint;
                   case 1:
                       msg = "setting Speed Control Mode";
                       m.changeControlMode(CANJaguar.ControlMode.kSpeed);
                       ++entryPoint;
                   case 2:
                       msg = "setting speed & pos ref";
                       m.setSpeedReference(CANJaguar.SpeedReference.kEncoder);
                       m.setPositionReference(CANJaguar.PositionReference.kQuadEncoder);
                       ++entryPoint;
                   case 3:
                       msg = "setting Encoder Lines";
                       m.configEncoderCodesPerRev(encoderLines[i]);
                       ++entryPoint;
                   case 4:
                       msg = "setting PID gains";
                       m.setPID(pidGains[i][0], pidGains[i][1], pidGains[i][2]);
                       ++entryPoint;
                   case 5:
                       msg = "enabling control";
                       m.enableControl();
                       ++entryPoint;
                       keepTrying = false;
                   default:
                       break;
               }
            } catch (Exception e){
                cl.log("Error configuring " + desc + " in step " + msg + ", retrying...");
                Timer.delay(.1);
            }
        }
        cl.log(desc + " configured successfully");
    }

    public void mecanumDrive(double forward, double turn, double rotate) {
    //Watchdog.getInstance().feed();
        if (++driveNow > 2)  {
            driveNow = 0;

            //if (forward > -0.01 && forward < 0.01) forward = 0;
            if (turn > -0.01 && turn < 0.01) turn = 0;
            //if (rotate > -0.01 && rotate < 0.01) rotate = 0;

            frontLeftMotorSpeed = forward + rotate + turn;
            frontRightMotorSpeed = forward - rotate - turn;
            rearLeftMotorSpeed = forward + rotate - turn;
            rearRightMotorSpeed = forward - rotate + turn;

            double max = Math.abs(frontLeftMotorSpeed);
            double temp = Math.abs(frontRightMotorSpeed);
            if (temp > max) max = temp;
            temp = Math.abs(rearLeftMotorSpeed);
            if (temp > max) max = temp;
            temp = Math.abs(rearRightMotorSpeed);
            if (temp > max) max = temp;

            if (max > 1.0) {
		frontLeftMotorSpeed /= max;
		frontRightMotorSpeed /= max;
		rearLeftMotorSpeed /= max;
		rearRightMotorSpeed /= max;
            }

//            if (frontLeftMotorSpeed < -.01 || frontLeftMotorSpeed > .01)
                frontLeftMotorSpeed = (minRPM + (frontLeftMotorSpeed * rangeRPM));
//            else frontLeftMotorSpeed = 0;
            if (frontRightMotorSpeed < -.01 || frontRightMotorSpeed > .01)
                frontRightMotorSpeed = (minRPM + (frontRightMotorSpeed * rangeRPM));
            else frontRightMotorSpeed = 0;
//            if (rearLeftMotorSpeed < -.01 || rearLeftMotorSpeed > .01)
                rearLeftMotorSpeed = (minRPM + (rearLeftMotorSpeed * rangeRPM));
//            else rearLeftMotorSpeed = 0;
//            if (rearRightMotorSpeed < -.01 || rearRightMotorSpeed > .01)
                rearRightMotorSpeed = (minRPM + (rearRightMotorSpeed * rangeRPM));
//            else rearRightMotorSpeed = 0;

            String msg = "";
            byte syncGroup = 0x20;
            try {
               
                msg = "frontLeftJag";
                frontLeftMotor.setX(-frontLeftMotorSpeed, syncGroup);
                msg = "frontRightJag";
                frontRightMotor.setX(frontRightMotorSpeed, syncGroup);
                msg = "rearLeftJag";
                rearLeftMotor.setX(-rearLeftMotorSpeed, syncGroup);
                msg = "rearRightJag";
                rearRightMotor.setX(rearRightMotorSpeed, syncGroup);
                msg = "syncGroup";
                CANJaguar.updateSyncGroup(syncGroup);
            } catch (Exception e) {
                cl.log("mecanumDrive error at step " + msg);
            }
            try
            {
                CANJaguar.updateSyncGroup(syncGroup);
            }
            catch (Exception e) {
                cl.log("mecanumDrive error at step " + msg);
        }
    }
    }

    public void mecanumDrive2(double forward, double turn, double rotate) {
    //Watchdog.getInstance().feed();
        if (++driveNow > 2)  {
            driveNow = 0;

            //if (forward > -0.01 && forward < 0.01) forward = 0;
            if (turn > -0.01 && turn < 0.01) turn = 0;
             if (forward > -0.01 && forward < 0.01) forward = 0;
             if (rotate > -0.01 && rotate < 0.01) rotate = 0;
            //if (rotate > -0.01 && rotate < 0.01) rotate = 0;

            frontLeftMotorSpeed = forward + rotate + turn;
            frontRightMotorSpeed = forward - rotate - turn;
            rearLeftMotorSpeed = forward + rotate - turn;
            rearRightMotorSpeed = forward - rotate + turn;

            double max = Math.abs(frontLeftMotorSpeed);
            double temp = Math.abs(frontRightMotorSpeed);
            if (temp > max) max = temp;
            temp = Math.abs(rearLeftMotorSpeed);
            if (temp > max) max = temp;
            temp = Math.abs(rearRightMotorSpeed);
            if (temp > max) max = temp;

            if (max > 1.0) {
		frontLeftMotorSpeed /= max;
		frontRightMotorSpeed /= max;
		rearLeftMotorSpeed /= max;
		rearRightMotorSpeed /= max;
            }

//            if (frontLeftMotorSpeed < -.01 || frontLeftMotorSpeed > .01)
                frontLeftMotorSpeed = (minRPM + (frontLeftMotorSpeed * rangeRPM));
//            else frontLeftMotorSpeed = 0;
            if (frontRightMotorSpeed < -.01 || frontRightMotorSpeed > .01)
                frontRightMotorSpeed = (minRPM + (frontRightMotorSpeed * rangeRPM));
            else frontRightMotorSpeed = 0;
//            if (rearLeftMotorSpeed < -.01 || rearLeftMotorSpeed > .01)
                rearLeftMotorSpeed = (minRPM + (rearLeftMotorSpeed * rangeRPM));
//            else rearLeftMotorSpeed = 0;
//            if (rearRightMotorSpeed < -.01 || rearRightMotorSpeed > .01)
                rearRightMotorSpeed = (minRPM + (rearRightMotorSpeed * rangeRPM));
//            else rearRightMotorSpeed = 0;

            String msg = "";
            byte syncGroup = 0x20;
            try {

                msg = "frontLeftJag";
                frontLeftMotor.setX(-frontLeftMotorSpeed, syncGroup);
                msg = "frontRightJag";
                frontRightMotor.setX(frontRightMotorSpeed, syncGroup);
                msg = "rearLeftJag";
                rearLeftMotor.setX(-rearLeftMotorSpeed, syncGroup);
                msg = "rearRightJag";
                rearRightMotor.setX(rearRightMotorSpeed, syncGroup);
                msg = "syncGroup";
                CANJaguar.updateSyncGroup(syncGroup);
            } catch (Exception e) {
                cl.log("mecanumDrive error at step " + msg);
            }
            try
            {
                CANJaguar.updateSyncGroup(syncGroup);
            }
            catch (Exception e) {
                cl.log("mecanumDrive error at step " + msg);
        }
    }
    }
    public StringTokenizer motorSPs() {
        String val = Double.toString(frontLeftMotorSpeed);
        String str = "SP FL: " + val;
        val = Double.toString(frontRightMotorSpeed);
        str += " FR: " + val;
        val = Double.toString(rearLeftMotorSpeed);
        str += "|RL: " + val;
        val = Double.toString(rearRightMotorSpeed);
        str += " RR: " + val + "   ";
        return new StringTokenizer(str, "|");
    }

    public StringTokenizer motorPVs() {
        String str = "PV FL: ";
        try {
            String val = Double.toString(frontLeftMotor.getSpeed());
            str += val;
            val = Double.toString(frontRightMotor.getSpeed());
            str += " FR: " + val;
            val = Double.toString(rearLeftMotor.getSpeed());
            str += "|RL: " + val;
            val = Double.toString(rearRightMotor.getSpeed());
            str += " RR: " + val + "   ";
        } catch (Exception e) {
            str = "PV: cannot read      |PV: cannot read      ";
        }
        return new StringTokenizer(str, "|");
    }

    private void updateGyroAndTemp() {
        currentTemp =(((Temperature.getAverageVoltage()-2.5)*200)+77);
        currentTemp = ((currentTemp-32)*5)/9;
        currentGyroAngle = Gyroscope.getAngle()+((currentTemp-startTemp)*2.5);
    }
public boolean spinInAcircle()
    {
    updateGyroAndTemp();
    if (getGyroAngle()>0)
    {
        mecanumDrive(0.0,0.0,.1);
    }
     if (getGyroAngle()<0)
    {
        mecanumDrive(0.0,0.0,-.1);
    }
    if (Math.abs(getGyroAngle())<1)
    {
        return true;
    }
    return false;
}
    public double getGyroAngle() {
        updateGyroAndTemp();
        return currentGyroAngle;
    }

    public double getCurTemp() {
        updateGyroAndTemp();
        return currentTemp;
    }

    public String getSwitches() {
        String msg =  "L: " + (lineTrackerLeft.get() ? "1" : "0");
        msg += " M: " + (lineTrackerMid.get() ? "1" : "0");
        msg += " R: " + (lineTrackerRight.get() ? "1" : "0");
        return msg;
    }

    // try changing so that DIRECTION is applied to x axis (turn) as opposed
    // to z axis (rotate).
    // or, if still applying DIRECTION to the rotation, consider straightening
    // out once only the middle sensor is on (current we keep driving at diagonal)
     double icing = 0;
     double awaybettervalue = 0;
    public boolean followLineYeah(double speed, double inc)
    {
        double pancakes = 0;
       
        double dawaffles = 0;

        boolean left, right, mid;
        left = !lineTrackerLeft.get();
        mid = !lineTrackerMid.get();
        right = !lineTrackerRight.get();
        String value = "";
        int abettervalue = 0;
       
        value=(left?"1":"0")+(mid?"1":"0")+(right?"1":"0");
       if (value.equals("111"))
       {
        abettervalue=0;
        }
        if (value.equals("110"))
       {
        abettervalue=-1;
        }
        if (value.equals("100"))
       {
        abettervalue=-2;
        }
         if (value.equals("001"))
       {
        abettervalue=2;
        }
        if (value.equals("011"))
       {
        abettervalue=1;
        }
        if ((value.equals("010")) || (value.equals("111")))
       {
        abettervalue=0;
        }
        if (value.equals("000")){
            if (pancakes == 0){
                abettervalue=0;
            }
            if(pancakes<0){
                abettervalue=-3;

            }
            if(pancakes>0){
                abettervalue=3;
            }
        }
        pancakes = (-abettervalue)*kp;
        icing+=-abettervalue;
        icing*=ki;
        dawaffles = (-abettervalue-yes)*kd;
        yes = -abettervalue;
        awaybettervalue = pancakes + icing + dawaffles;
mecanumDrive(-.10,0, awaybettervalue);
        
        
        

return false;
    }
    double yes = 0;
public double kp = 0;
public double ki = 0;
public double kd = 0;


    public boolean followLine(double speed,double inc)
    {
        boolean left, right, mid;
        left = !lineTrackerLeft.get();
        mid = !lineTrackerMid.get();
        right = !lineTrackerRight.get();

        double DIRECTION = 0;
        boolean atEndOfLine = false;
        if (mid)//Don't Panic, we are on the line!
        {
            if (left && !right)//we are leaning off center line towards the right, come left to correct
            {
                DIRECTION+=inc;
            }
            if (!left && right)//we are leaning off center line towards the left, come right to correct
            {
                DIRECTION-=inc;
            }
            if (left && right)//We are at the T,stop
            {
                speed = 0;
                atEndOfLine = true;
            }
            if (!left && !right)//We're centered on the line
            {
                //I'll need to know our direction...
                //System.out.println(""+left+","+mid+","+right+": go straight");
               //DIRECTION = 0;
            }
        }
        else
        {
            if (left && !right)//Leaning towards the left a lot?
            {
                //DIRECTION = Math.min(DIRECTION,0);
                DIRECTION+=(inc*2);
              //  System.out.println(""+left+","+mid+","+right+": decrease direction, turn left");
                //speed /= 2;

            }
            if (!left && right)//Leaning towards the right a lot?
            {
                //DIRECTION = Math.max(DIRECTION,0);
                DIRECTION-=(inc*2);
            //    System.out.println(""+left+","+mid+","+right+": increase direction, turn right");
               //speed /= 2;
            }
            if (left && right)//We are at the fork!
            {
                endOfLine = true;
                //I'll need to decide... for now, lets go left
                //DIRECTION+=inc;
                //System.out.println(""+left+","+mid+","+right+": do nothing");
              //speed /= 2;
            }
            if (!left && !right)//We are completely lost...
            {
                //isOnLine = no;
                //return;
                //System.out.println(""+left+","+mid+","+right+": do nothing");
            }
        }
        mecanumDrive(-speed,0, DIRECTION);
        return atEndOfLine;
    }

    private String getMotorSpeed(CANJaguar m) {
        try { return Double.toString(m.getSpeed()); }
        catch (Exception e) { return "ERR"; }
    }

    private String getMotorPos(CANJaguar m) {
        try { return Double.toString(m.getPosition()); }
        catch (Exception e) { return "ERR"; }
    }

    private String getMotorBusV(CANJaguar m) {
        try { return Double.toString(m.getBusVoltage()); }
        catch (Exception e) { return "ERR"; }
    }

    private String getMotorFaults(CANJaguar m) {
        try { return Integer.toBinaryString(m.getFaults()); }
        catch (Exception e) { return "ERR"; }
    }

    public void saveStartingPosition() {
        try {
            startingPosition = Math.abs(frontLeftMotor.getPosition()) * wheelInchesPerRev;
        } catch (Exception e) {
            startingPosition = Double.NaN;
        }
    }

    public double getDistanceTravelled() {
        double retValue;
        if (startingPosition == Double.NaN) {
            retValue = Double.NaN;
        }
        else {
            try {
                retValue = Math.abs(frontLeftMotor.getPosition()) * wheelInchesPerRev;
                retValue -= startingPosition;
            } catch(Exception e) {
                retValue = Double.NaN;
            }
        }
        return retValue;
    }

    public boolean driveToZone(double speed) {
        mecanumDrive(speed,0.0,0.0);
        boolean left, right, mid;
        left = !lineTrackerLeft.get();
        mid = !lineTrackerMid.get();
        right = !lineTrackerRight.get();
        return (left && mid && right);
    }

    public String getTelemetryHeader() {
        String val = "FL SP,FR SP,RL SP,RR SP,";
        val += "StartPos,";
        //val += "FL PV,FR PV,RL PV,RR PV,";
        //val += "FL Pos,FR Pos,RL Pos,RR Pos,";
        //val += "FL BusV,FR BusV,RL BusV,RR BusV,";
        //val += "FL Fault,FR Fault,RL Fault,RR Fault,";
        val += "Gyro,Left,Mid,Right,";
        val += "Enc1,Enc2,Enc3,Enc4";
        return val;
    }

    public String getTelemetryData() {
       
            String val = Double.toString(frontLeftMotorSpeed) + ",";
            val += Double.toString(frontRightMotorSpeed) + ",";
            val += Double.toString(rearLeftMotorSpeed) + ",";
            val += Double.toString(rearRightMotorSpeed) + ",";
            val += Double.toString(startingPosition) + ",";
            /*val += getMotorSpeed(frontLeftMotor) + ",";
            val += getMotorSpeed(frontRightMotor) + ",";
            val += getMotorSpeed(rearLeftMotor) + ",";
            val += getMotorSpeed(rearRightMotor) + ",";
            val += getMotorPos(frontLeftMotor) + ",";
            val += getMotorPos(frontRightMotor) + ",";
            val += getMotorPos(rearLeftMotor) + ",";
            val += getMotorPos(rearRightMotor) + ",";
            val += getMotorBusV(frontLeftMotor) + ",";
            val += getMotorBusV(frontRightMotor) + ",";
            val += getMotorBusV(rearLeftMotor) + ",";
            val += getMotorBusV(rearRightMotor) + ",";
            val += getMotorFaults(frontLeftMotor) + ",";
            val += getMotorFaults(frontRightMotor) + ",";
            val += getMotorFaults(rearLeftMotor) + ",";
            val += getMotorFaults(rearRightMotor) + ",";
             */
            val += Double.toString(getGyroAngle()) + ",";
            val += (lineTrackerLeft.get() ? "Off Line" : "On Line") + ",";
            val += (lineTrackerMid.get() ? "Off Line" : "On Line") + ",";
            val += (lineTrackerRight.get() ? "Off Line" : "On Line")+ ",";
             try {
            val += (frontLeftMotor.getPosition())+ ",";
            val += (frontRightMotor.getPosition())+ ",";
            val += (rearLeftMotor.getPosition())+ ",";
            val += (rearRightMotor.getPosition());

            
        }
        catch (CANTimeoutException ex) {
            ex.printStackTrace();
        }
        return val;
    }

    public LogomotionDriveTrain() {

        driveNow = 0;
        initMotors();
        configMotor(frontLeftMotor, "frontLeftJag", frontLeftIndex);
        configMotor(frontRightMotor, "frontRightJag", frontRightIndex);
        configMotor(rearLeftMotor, "rearLeftJag", rearLeftIndex);
        configMotor(rearRightMotor, "rearRightJag", rearRightIndex);
        saveStartingPosition();
        lineTrackerLeft = new DigitalInput(leftPhotoSwitchDI);
        lineTrackerMid = new DigitalInput(midPhotoSwitchDI);
        lineTrackerRight = new DigitalInput(rightPhotoSwitchDI);
        Gyroscope = new Gyro(gyroAI);
        Temperature = new AnalogChannel(temperatureAI);
        startTemp = (((Temperature.getVoltage()-2.5)*200)+77);
        startTemp = ((startTemp-32)*5)/9;
        resetGyro();
        updateGyroAndTemp();
        StickWithIt = 0;
    }

}
